#include "cv.h"
#include <highgui.h>
#include "opencv2/imgproc/imgproc.hpp"
#include "cv_bridge/cv_bridge.h"
#include <image_transport/image_transport.h>

#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Int8MultiArray.h>

#include <string.h>
#include <signal.h>

using namespace cv;


/*
This node is doing :
	-> Receive images with only the tag from tag detector
	-> Resize it and make it binary
	-> Save it as a template
*/


/* Function to receive the image */
void imageCallbackCam0(const sensor_msgs::ImageConstPtr& image_msg)
{
	Mat imgMat;
	cv_bridge::CvImagePtr cv_ptr;
	try
	{
		cv_ptr = cv_bridge::toCvCopy(image_msg, "bgr8");
		imgMat = cv_ptr->image;
	}
	catch (cv_bridge::Exception& e)
	{
		ROS_ERROR("cv_bridge exception: %s", e.what());
	return;
	}
	printf("Tag received \n");

	Mat imgrey, binary;
	cvtColor(imgMat,imgrey,CV_RGB2GRAY);
	threshold(imgrey, binary, 80, 255, THRESH_BINARY);
	
	Mat templateImg;
	resize(binary,templateImg,Size(60,60),0,0,INTER_LINEAR);

	namedWindow( "Template", 1 );
    	imshow( "Template", templateImg);
    	waitKey(0);
	imwrite("teddy_3.jpg",templateImg);
	
}


int main(int argc, char **argv)
{

	ros::init(argc, argv, "TemplateImages");
	printf("Creating template images \n");
	ros::NodeHandle n;

	image_transport::ImageTransport it(n);
	image_transport::Subscriber sub_imgCam0 = it.subscribe("camera/imageCam0", 1, imageCallbackCam0);

	ros::spin();
}
